function [latLonAlt] = levarmcomp_pos(levArmLatLonAlt, CBN, lB, f, RE) %#codegen
%LEVARMCOMP_POS 简化的位置杆臂补偿方法
%
% Tips:
% # 导航系N初始为ENU
%
% Input Arguments:
% # levArmLatLonAlt: 3*1向量，分别为杆臂端点位置的纬度、经度、高度，单位为rad、rad、m
% # CBN: 3*3矩阵，姿态矩阵，无单位
% # lB: 3*1向量，体系下的杆臂（从输出位置到输入位置），单位m
% # f: 标量，地球模型椭球体的扁率，无单位
% # RE: 标量，地球模型椭球体的半长轴，单位m
%
% Output Arguments:
% # latLonAlt: 3*1向量，分别为纬度、经度、高度，单位为rad、rad、m

lG = CBN*lB;
R = norm(lla2ecef_cg(levArmLatLonAlt', f, RE));
latLonAlt = levArmLatLonAlt - [lG(2, 1)/R; lG(1, 1)/R/cos(levArmLatLonAlt(1, 1)); lG(3, 1)];
end